#include"vld_imu_slam/backend.h"

namespace vld_imu_slam{

void Backend::lidarOdomBackendOpt(KeyFramesType& keyframes,vector<LoopFactor>& loopfactor_v){
    addOdomFactor(keyframes);
    // 执行优化
    isam_->update(gtSAMgraph_, initialEstimate_);
    isam_->update();

    if (addLoopFactor(loopfactor_v))
    {
        isam_->update();
        isam_->update();
        isam_->update();
        isam_->update();
        isam_->update();
    }

    // update之后要清空一下保存的因子图，注：历史数据不会清掉，ISAM保存起来了
    gtSAMgraph_.resize(0);
    initialEstimate_.clear();

    // 优化结果
    auto isamCurrentEstimate = isam_->calculateEstimate();
    for(int i=0;i<keyframes.size();i++){
        auto latestEstimate = isamCurrentEstimate.at<gtsam::Pose3>(i);
        keyframes[i]->setPose(gtsamPose2SE3(latestEstimate));
    }
    ROS_DEBUG_STREAM("backend opt current "<<keyframes[keyframes.size()-1]->time_stamp_
                <<" frame: "<<keyframes[keyframes.size()-1]->frame_id_
                << " pose: "<<SE3toEigenM( keyframes[keyframes.size()-1]->getPose()));
}

void Backend::addOdomFactor(KeyFramesType& keyframes){
    if (keyframes.size()==1)
    {
        // 第一帧初始化先验因子
        gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances((gtsam::Vector(6) << 1e-2, 1e-2, M_PI*M_PI, 1e8, 1e8, 1e8).finished()); // rad*rad, meter*meter
        gtSAMgraph_.add(gtsam::PriorFactor<gtsam::Pose3>(0, SE3togtsamPose(keyframes[0]->getPose()), priorNoise));
        // 变量节点设置初始值
        initialEstimate_.insert(0, SE3togtsamPose(keyframes[0]->getPose()));
    }else{
        // 添加激光里程计因子
        gtsam::noiseModel::Diagonal::shared_ptr odometryNoise = gtsam::noiseModel::Diagonal::Variances((gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
        gtsam::Pose3 poseFrom = SE3togtsamPose(keyframes[keyframes.size()-2]->getPose());
        gtsam::Pose3 poseTo   = SE3togtsamPose(keyframes[keyframes.size()-1]->getPose());
        // 参数：前一帧id，当前帧id，前一帧与当前帧的位姿变换（作为观测值），噪声协方差
        gtSAMgraph_.add(gtsam::BetweenFactor<gtsam::Pose3>(keyframes.size()-2, keyframes.size()-1, poseFrom.between(poseTo), odometryNoise));
        // 变量节点设置初始值
        initialEstimate_.insert(keyframes.size()-1, poseTo);
    }
}

bool Backend::addLoopFactor(vector<LoopFactor>& loopfactor_v)
{
    if (loopfactor_v.empty())
        return false;
    ROS_DEBUG_STREAM("backend loop detected! ");
    // 闭环队列
    for (auto loopfactor:loopfactor_v)
    {
        gtSAMgraph_.add(gtsam::BetweenFactor<gtsam::Pose3>(loopfactor.cur_idx, loopfactor.loop_idx, loopfactor.T_cur_loopframe, loopfactor.constraintNoise));
    }
    loopfactor_v.clear();
    return true;
}

}